// Copyright FYT 邹承甫 2022

#ifndef POST_PROCESSOR__PROCESSOR_NODE_HPP_
#define POST_PROCESSOR__PROCESSOR_NODE_HPP_

#include <string>
#include <vector>

#include <dynamic_reconfigure/server.h>
#include <message_filters/subscriber.h>
#include <ros/ros.h>
#include <std_msgs/Time.h>
#include <tf2_ros/message_filter.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <Eigen/Core>

#include "configures/ProcessorConfig.h"
#include "rm_interfaces/AllTargets.h"
#include "rm_interfaces/Point2d.h"
#include "rm_interfaces/SerialReceiveMsg.h"

#include "armor.hpp"
#include "pnp_solver.hpp"
#include "predictor.hpp"
#include "tracker.hpp"

namespace post_processor {
class ProcessorNode {
  using config_srv = dynamic_reconfigure::Server<processor::ProcessorConfig>;
  using target_filter = message_filters::Subscriber<rm_interfaces::AllTargets>;
  using tf2_filter = tf2_ros::MessageFilter<rm_interfaces::AllTargets>;

 public:
  ProcessorNode();

 private:
  void targetsCallback(const rm_interfaces::AllTargetsConstPtr &targets);

  std::vector<Armor> solveArmorPoses(
      const rm_interfaces::AllTargetsConstPtr &targets);

  void serialCallback(const rm_interfaces::SerialReceiveMsgConstPtr &msg);

  void publishMsg(const float goal_yaw, const float goal_pitch,
                  const float distance);

  void initMarkers();

  void publishEmptyMarkers();

  void publishMarkers(const Eigen::VectorXd &predict_state,
                      const Eigen::VectorXd &track_state,
                      const std::vector<Armor> &armors);

  void configure_cb(const processor::ProcessorConfig &config, uint32_t level);

  void publishGoalAxisTf(double yaw, double pitch);

  ros::NodeHandle nh_;
  ros::Publisher pub_;
  ros::Publisher info_pub_;
  ros::Publisher vis_pub_;
  ros::Subscriber serial_sub_;
  target_filter targets_sub_;
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listenner_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> goal_axis_broadcaster_;
  std::shared_ptr<tf2_filter> tf_filter_;

  config_srv config_server_;
  config_srv::CallbackType f_;

  PnPSolver::Ptr pnp_solver_;
  Tracker::Ptr tracker_;
  Predictor::Ptr predictor_;

  visualization_msgs::Marker position_marker_;
  visualization_msgs::Marker predict_marker_;
  visualization_msgs::Marker velocity_marker_;
  visualization_msgs::Marker aim_shoot_marker_;
  visualization_msgs::Marker text_marker_;

  ros::Time time_now_;
  std::string target_frame_;
  std::string camera_frame_;
  Eigen::Vector3d translation_;
  float yaw_;
  float pitch_;
  bool debug_;
};

}  // namespace post_processor
#endif  // POST_PROCESSOR__PROCESSOR_NODE_HPP_